#include <actionlib/client/simple_action_client.h>
#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value
#include <move_base_msgs/MoveBaseAction.h>
#include <nav_msgs/OccupancyGrid.h>
#include <queue>
#include <ros/console.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>

using namespace std;
class MapProcessor {

private:
   typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
   enum WFD {
      MOL, MCL, FOL, FCL, NONE
   };
   enum FrontierSelection {
      CLOSEST, FARTHEST, LARGEST, RANDOM
   };
   MoveBaseClient ac;
   unsigned int height;
   tf::TransformListener listener;
   ros::Subscriber mapSub; // Subscriber to gmappings map topic
   float resolution;
   unsigned int width;

   vector<unsigned int> getNeighbors(unsigned int index) const {
      vector<unsigned int> neighbors;
      bool up, right, down, left;
      up = index >= width;
      right = (index % width) < (width - 1);
      down = index < (height - 1) * width;
      left = (index % width) > 0;

      if (up)
         neighbors.push_back(index - width);
      if (up && right)
         neighbors.push_back(index - width + 1);
      if (right)
         neighbors.push_back(index + 1);
      if (down && right)
         neighbors.push_back(index + width + 1);
      if (down)
         neighbors.push_back(index + width);
      if (down && left)
         neighbors.push_back(index + width + 1);
      if (left)
         neighbors.push_back(index - 1);
      if (up && left)
         neighbors.push_back(index - width - 1);

      return neighbors;
   }

   bool bordersOpen(const nav_msgs::OccupancyGridConstPtr& msg, unsigned int index) const {
      vector<unsigned int> neighbors = getNeighbors(index);

      for (vector<unsigned int>::iterator it = neighbors.begin(); it != neighbors.end(); ++it) {
         if (msg->data[*it] == 0)
            return true;
      }

      return false;
   }

   bool isFrontierCell(const nav_msgs::OccupancyGridConstPtr& msg, unsigned int index) const {
      if (msg->data[index] == -1) {
         return bordersOpen(msg, index);
      }

      return false;
   }

public:
   MapProcessor(ros::NodeHandle& nh) :
         ac("move_base", true), height(0), resolution(-1), width(0) {
      // Initialize random time generator
      srand(time(NULL));

      // Subscribe to gmappings map
      mapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapProcessor::gridCallback, this, _1));

      //wait for the action server to come up
      while (!ac.waitForServer(ros::Duration(5.0))) {
         ROS_INFO("Waiting for the move_base action server to come up");
      }
   }

// Process the incoming message
   void gridCallback(const nav_msgs::OccupancyGridConstPtr& msg) {
      // Incoming messages are of type nav_msgs::OccupancyGrid. Their format is listed below.
      // The actual map is stored as a 1D int8 array.
      //      Header header
      //          uint32 seq
      //          time stamp
      //          string frame_id
      //      MapMetaData info
      //          time map_load_time
      //          float32 resolution
      //          uint32 width
      //          uint32 height
      //          geometry_msgs/Pose origin
      //              geometry_msgs/Point position
      //                  float64 x
      //                  float64 y
      //                  float64 z
      //              geometry_msgs/Quaternion orientation
      //                  float64 x
      //                  float64 y
      //                  float64 z
      //                  float64 w
      //      int8[] data

      // Gmapping might resize the map. Look out for it.
      if (msg->info.height != height || msg->info.width != width) {
         height = msg->info.height;
         resolution = msg->info.resolution;
         width = msg->info.width;
      }

      vector<vector<unsigned int> > frontiers;
      vector<WFD> indicator(msg->data.size(), NONE);
      queue<unsigned int> queue_m;
      queue<unsigned int> queue_f;

      // Retrieve turtle position
      tf::StampedTransform turtle_tf;
      try {
         listener.lookupTransform("/map", "/base_link", ros::Time(0), turtle_tf);
      } catch (tf::TransformException& ex) {
         ROS_ERROR("%s", ex.what());
      }

      int x = static_cast<int>(turtle_tf.getOrigin().x() / resolution) + (width / 2);
      int y = static_cast<int>(turtle_tf.getOrigin().y() / resolution) + (height / 2);
      unsigned int pose = x + y * width;

      cout << "[INFO] Turtle at: " << turtle_tf.getOrigin().x() << ", " << turtle_tf.getOrigin().y() << " => " << pose << endl;

      // Start of WFD
      queue_m.push(pose);
      indicator[pose] = MOL;

      while (!queue_m.empty()) {
         unsigned int p = queue_m.front();
         queue_m.pop();

         if (indicator[p] == MCL)
            continue;
         if (isFrontierCell(msg, p)) {
            queue<unsigned int> queue_f;
            vector<unsigned int> frontier;
            queue_f.push(p);
            indicator[p] = FOL;

            while (!queue_f.empty()) {
               unsigned int q = queue_f.front();
               queue_f.pop();
               if (indicator[q] == MCL || indicator[q] == FCL)
                  continue;
               if (isFrontierCell(msg, q)) {
                  frontier.push_back(q);
                  vector<unsigned int> neighbors = getNeighbors(q);
                  for (vector<unsigned int>::iterator it = neighbors.begin(); it != neighbors.end(); ++it) {
                     if (indicator[*it] != FOL && indicator[*it] != FCL && indicator[*it] != MCL) {
                        queue_f.push(*it);
                        indicator[*it] = FOL;
                     }
                  }
               }
               indicator[q] = FCL;
            }
            frontiers.push_back(frontier);
            for (vector<unsigned int>::iterator it = frontier.begin(); it != frontier.end(); ++it) {
               indicator[*it] = MCL;
            }
         }
         vector<unsigned int> neighbors = getNeighbors(p);
         for (vector<unsigned int>::iterator it = neighbors.begin(); it != neighbors.end(); ++it) {
            if (indicator[*it] != MOL && indicator[*it] != MCL && bordersOpen(msg, *it)) {
               queue_m.push(*it);
               indicator[*it] = MOL;
            }
         }
         indicator[p] = MCL;
      }
      // End of WFD

      // Frontier selection parameters
      // Selection method can be chosen from: CLOSEST, FARTHEST, LARGEST, RANDOM.
      FrontierSelection method = LARGEST;
      unsigned int frontierSize = 0;
      unsigned int closestFrontier = UINT_MAX;
      unsigned int farthestFrontier = 0;
      int randomSelectionThreshold = 10;

      bool frontierSelected = false;
      double frontierX = 0;
      double frontierY = 0;

      // Find the 'best' frontier
      for (vector<vector<unsigned int> >::iterator it = frontiers.begin(); it != frontiers.end(); ++it) {
         // Determine average X and Y coordinates for this frontier
         double xAvg = 0;
         double yAvg = 0;
         for (vector<unsigned int>::iterator i = (*it).begin(); i != (*it).end(); ++i) {
            xAvg += *i % height;
            yAvg += *i / width;
         }
         xAvg /= (*it).size();
         yAvg /= (*it).size();

         // Distance from turtle's current position
         double xDis = turtle_tf.getOrigin().x() - resolution * (xAvg - (height / 2));
         double yDis = turtle_tf.getOrigin().y() - resolution * (yAvg - (width / 2));
         double dis = sqrt(xDis * xDis + yDis * yDis); // Pythagoras should be good enough for now

         if (method == LARGEST) {
            if ((*it).size() > frontierSize) {
               frontierSize = (*it).size();
               frontierX = xAvg;
               frontierY = yAvg;
               frontierSelected = true;
            }
         } else if (method == CLOSEST) {
            if (closestFrontier > dis) {
               closestFrontier = dis;
               frontierX = xAvg;
               frontierY = yAvg;
               frontierSelected = true;
            }
         } else if (method == FARTHEST) {
            if (farthestFrontier < dis) {
               farthestFrontier = dis;
               frontierX = xAvg;
               frontierY = yAvg;
               frontierSelected = true;
            }
         } else if (method == RANDOM) {
            // To be sure we always do select one frontier in random mode, select first option and adapt later
            if (frontierSelected == false) {
               frontierX = xAvg;
               frontierY = yAvg;
               frontierSelected = true;
            } else if (rand() % 100 < randomSelectionThreshold) {
               frontierX = xAvg;
               frontierY = yAvg;
            }

         }
      }

      // Generate message with destination for navigation stack
      move_base_msgs::MoveBaseGoal goal;
      frontierX = resolution * (frontierX - (width / 2));
      frontierY = resolution * (frontierY - (height / 2));

      // Send turtlebot to goal coordinates
      goal.target_pose.header.frame_id = "map";

      goal.target_pose.header.stamp = ros::Time::now();
      goal.target_pose.pose.position.x = frontierX;
      goal.target_pose.pose.position.y = frontierY;
      goal.target_pose.pose.position.z = 0;
      // TODO Figure out a proper orientation
      goal.target_pose.pose.orientation.w = 1;
      cout << "[INFO] Moving to: " << frontierX << ", " << frontierY << endl;

      // Send turtle to new goal coordinates
      ac.sendGoal(goal);

      // This function will not return before the goal is reached, the attempt is aborted,
      // or the timeout limit of 90 seconds is reached
      ac.waitForResult(ros::Duration(90.0));

      // Check whether or not the goal is reached
      if (ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
         ROS_INFO("Oops! We failed to move to the goal coordinates for some reason.");
   }
}
;

int main(int argc, char **argv) {
   // Initiate new ROS node named "turtle_map_processor"
   ros::init(argc, argv, "turtle_map_processor");
   ros::NodeHandle n;
   // Create new map processor object
   MapProcessor mp(n);

   ros::spin();
   return 0;
}
